
#include <stdio.h>
#include <stdint.h>
#include <inttypes.h>
#include <stdlib.h>
#include <math.h>

extern void render_gradient_soft(uint8_t* pixel, int w, int h, int pitch);
extern void render_project_depth_soft(uint8_t* pixel, int w, int h, int pitch);

#include <time.h>
uint64_t now_ms(void)
{
    struct timespec tspec;
    clock_gettime(CLOCK_MONOTONIC, &tspec);

    return tspec.tv_sec * 1000 + tspec.tv_nsec / 1000000;
}

#include <bcm_host.h>

int main(int argc, char *argv[])
{
    DISPMANX_DISPLAY_HANDLE_T display;
    DISPMANX_MODEINFO_T display_info;

    DISPMANX_ELEMENT_HANDLE_T element;

    VC_IMAGE_TYPE_T pix_fmt = VC_IMAGE_ARGB8888;
    int img_width = 640;
    int img_height = 480;
    int img_pitch = img_width * 4;
    VC_RECT_T image_rect_display = {0, 0, (img_width<<16), (img_height<<16)};
    VC_RECT_T image_rect = {0, 0, img_width, img_height};

    VC_DISPMANX_ALPHA_T alpha = { DISPMANX_FLAGS_ALPHA_FROM_SOURCE | DISPMANX_FLAGS_ALPHA_FIXED_ALL_PIXELS, 250, /*alpha 0->255*/ 0 };
    
    DISPMANX_RESOURCE_HANDLE_T img_resource;
    uint32_t image_handle;
    uint8_t *image_frame;

    bcm_host_init();

    display = vc_dispmanx_display_open(0);
    vc_dispmanx_display_get_info(display, &display_info);

    img_resource = vc_dispmanx_resource_create(pix_fmt, img_width, img_height, &image_handle);
    image_frame = (uint8_t*)malloc(img_pitch * img_height);

    /* render it */
    render_gradient_soft(image_frame, img_width, img_height, img_pitch);

    /* display it */
    {
        vc_dispmanx_resource_write_data(img_resource, pix_fmt, img_pitch, image_frame, &image_rect);

        DISPMANX_UPDATE_HANDLE_T update = vc_dispmanx_update_start(0);
        VC_RECT_T display_rect = {
            (display_info.width - img_width) / 2, 
            (display_info.height - img_height) / 2,
            img_width, img_height
        };
        element = vc_dispmanx_element_add(update, display, 0, 
            &display_rect, img_resource, &image_rect_display, 
            DISPMANX_PROTECTION_NONE, &alpha, NULL, DISPMANX_NO_ROTATE);
        vc_dispmanx_update_submit_sync(update);
    }

    getchar();

    render_project_depth_soft(image_frame, img_width, img_height, img_pitch);

    /* display it */
    {
        vc_dispmanx_resource_write_data(img_resource, pix_fmt, img_pitch, image_frame, &image_rect);

        DISPMANX_UPDATE_HANDLE_T update = vc_dispmanx_update_start(0);
        vc_dispmanx_element_remove(update, element);

        VC_RECT_T display_rect = {
            (display_info.width - img_width) / 2, 
            (display_info.height - img_height) / 2,
            img_width, img_height
        };
        element = vc_dispmanx_element_add(update, display, 0, 
            &display_rect, img_resource, &image_rect_display, 
            DISPMANX_PROTECTION_NONE, &alpha, NULL, DISPMANX_NO_ROTATE);
        vc_dispmanx_update_submit_sync(update);
    }

    getchar();

    {
        DISPMANX_UPDATE_HANDLE_T update = vc_dispmanx_update_start(0);
        
        vc_dispmanx_update_submit_sync(update);
    }

    free(image_frame);
    vc_dispmanx_resource_delete(img_resource);
    vc_dispmanx_display_close(display);

    bcm_host_deinit();

    return 0;
}
